/*
frigOS
Licensed under GPLv3
*/

#include "usart1.h"
#include "serial.h"
#include "adc.h"
#include "ax12.h"
#include "axs1.h"
#include "cm5.h"
#include "led.h"
#include "protocol.h"
#include "kernel.h"
#include "shared_memory.h"

#include <avr/interrupt.h>
#include <util/delay.h>

volatile uint8_t rxBuffer_USART1[USART1_RX_BUFFER_SIZE];
volatile uint8_t txBuffer_USART1[USART1_TX_BUFFER_SIZE];
volatile Semaphore usart1_tx_lock;
volatile Semaphore usart1_rx_lock;

volatile IO_Queue rxQueue_USART1;
volatile IO_Queue txQueue_USART1;

// initialize the PC-to-CM5 serial connection
// user may specify baud rade and optionally double the Tx speed
// if necessary
void initUSART1(Usart1_Frequency baud, BOOL doubleTx)
{
    // double usart transmission speed if specified
    if(doubleTx)
        UCSR1A = (1 << U2X1);
    else
        UCSR1A = 0x00;

    // set the baud rate
    switch(baud)
    {
    case USART1_9600_BAUD:
        if(doubleTx)
        {
            UBRR1H = (uint8_t) (USART1_BAUD_RATE_9600_U2X11 >> 8);
            UBRR1L = (uint8_t) USART1_BAUD_RATE_9600_U2X11;
        }
        else
        {
            UBRR1H = (uint8_t) (USART1_BAUD_RATE_9600_U2X10 >> 8);
            UBRR1L = (uint8_t) USART1_BAUD_RATE_9600_U2X10;
        }
        break;

    case USART1_115200_BAUD:
        if(doubleTx)
        {
            UBRR1H = (uint8_t) (USART1_BAUD_RATE_115200_U2X11 >> 8);
            UBRR1L = (uint8_t) USART1_BAUD_RATE_115200_U2X11;
        }
        else
        {
            UBRR1H = (uint8_t) (USART1_BAUD_RATE_115200_U2X10 >> 8);
            UBRR1L = (uint8_t) USART1_BAUD_RATE_115200_U2X10;
        }
        break;

    case USART1_57600_BAUD:
    default:
        if(doubleTx)
        {
            UBRR1H = (uint8_t) (USART1_BAUD_RATE_57600_U2X11 >> 8);
            UBRR1L = (uint8_t) USART1_BAUD_RATE_57600_U2X11;
        }
        else
        {
            UBRR1H = (uint8_t) (USART1_BAUD_RATE_57600_U2X10 >> 8);
            UBRR1L = (uint8_t) USART1_BAUD_RATE_57600_U2X10;
        }
        break;
    }


    // rx complete interrupt
    // tx complete interrupt
    // receiver
    // transmitter
    UCSR1B = (1 << RXCIE1) | (1 << TXCIE1) | (1 << RXEN1) | (1 << TXEN1);

    // 8N1
    // asynchronous
    // parity disabled
    // 1 stop bit
    // character size 8 bit
    UCSR1C = (1 << UCSZ11) | (1 << UCSZ10);

    reply_status = REPLY_ON;

    initSemaphore(&usart1_tx_lock, 1);
    initSemaphore(&usart1_rx_lock, 1);

    // initialize the rx and tx queues
    rxQueue_USART1.buffer = rxBuffer_USART1;
    rxQueue_USART1.maxLength = USART1_RX_BUFFER_SIZE;
    rxQueue_USART1.queueLength = 0;
    rxQueue_USART1.empty = TRUE;
    rxQueue_USART1.full = FALSE;
    rxQueue_USART1.txOnEnqueue = FALSE;
    rxQueue_USART1.in = 0;
    rxQueue_USART1.out = 0;
    rxQueue_USART1.txOn = TX_ON_NONE;

    txQueue_USART1.buffer = txBuffer_USART1;
    txQueue_USART1.maxLength = USART1_TX_BUFFER_SIZE;
    txQueue_USART1.queueLength = 0;
    txQueue_USART1.empty = TRUE;
    txQueue_USART1.full = FALSE;
    txQueue_USART1.txOnEnqueue = TRUE;
    txQueue_USART1.in = 0;
    txQueue_USART1.out = 0;
    txQueue_USART1.txOn = TX_ON_USART1;

} // initUSART1

// queue an array of bytes for transmission
void transmitBufferUSART1(uint8_t const buffer[], uint8_t buffer_length)
{
    uint8_t x;
    for (x = 0; x < buffer_length; x++)
    {
        enqueue(buffer[x], &txQueue_USART1);
    }
} // transmitBufferUSART1

// queue a string for transmission
// the NULL terminator is NOT transmitted
void transmitStringUSART1(const char *msg)
{
    uint8_t x;
    for (x = 0; msg[x] != 0; x++)
    {
        enqueue((uint8_t)msg[x], &txQueue_USART1);
    }
}

void transmitPacketUSART1(DXL_Packet *packet)
{
    uint8_t i;

    enqueue(packet->header1, &txQueue_USART1);
    enqueue(packet->header2, &txQueue_USART1);
    enqueue(packet->id, &txQueue_USART1);
    enqueue(packet->paramLength+2, &txQueue_USART1);
    enqueue(packet->instrErr, &txQueue_USART1);

    for(i=0; i<packet->paramLength; i++)
        enqueue(packet->params[i], &txQueue_USART1);

    enqueue(packet->checksum, &txQueue_USART1);
}

void transmitByteUSART1_busyLoop(char msg)
{
    txdLEDOn();
    // wait for the buffer to empty
    while( !(UCSR1A & (1 << UDRE1)) )
        asm volatile("nop"::);

    // send the byte
    UDR1 = msg;
    txdLEDOff();
}

// transmit a buffer over USART1 with a busy loop instead of relying on interrupts
// this bypasses the normal USART1 queue and should ONLY be used during initialization
// or debugging when interrupts are disabled
void transmitBufferUSART1_busyLoop(uint8_t const buffer[], uint8_t buffer_length)
{
    uint8_t x;
    for(x=0; x < buffer_length; x++)
    {
        transmitByteUSART1_busyLoop(buffer[x]);
    }
}

// transmit a string over USART1 with a busy loop instead of relying on interrupts
// this bypasses the normal USART1 queue and should ONLY be used during initialization
// or debugging when interrupts are disabled
void transmitStringUSART1_busyLoop(const char* msg)
{
    uint8_t x;
    for(x=0; msg[x] != 0; x++)
    {
        transmitByteUSART1_busyLoop(msg[x]);
    }
}

void transmitASCIIBufferUSART1_busyLoop(uint8_t const buffer[], uint8_t buffer_length)
{
    char hex[2];
    uint8_t i;

    for(i=0; i<buffer_length; i++)
    {
        byte2hex(buffer[i], hex);
        transmitBufferUSART1_busyLoop((uint8_t*)hex, 2);
        transmitByteUSART1_busyLoop(' ');
    }
}

// queue a single byte for transmission
void transmitByteUSART1(uint8_t msg)
{
    enqueue(msg, &txQueue_USART1);
}

// dequeue a byte from the Rx queue
uint8_t getByteUSART1()
{
    return dequeue(&rxQueue_USART1);
}

// listen for a single byte on USART1 and deal with it
// this code was initially in the USART1 Rx ISR, but moved here & fixed up
// to work with the new circular queues
// expects a standard Dynamixel packet (FF FF id length instruction/error params... checksum)
// if there's an error we send a response packet back with the appropriate error code
void monitorUSART1(void* arg)
{
    static uint8_t state = USART1_INITIAL_STATE;

    // format is a dynamixel packet, so store the data in an appropriate struct
    DXL_Packet packet;
    uint8_t paramsRead;
    uint8_t data;

    for(;;)
    {
        if(!rxQueue_USART1.empty)
        {
            lockSemaphore(&usart1_rx_lock);
            data = getByteUSART1();

            switch(state)
            {
            case USART1_INITIAL_STATE:
                if(data==DXL_HEADER1)
                {
                    state = USART1_HEADER1_STATE;
                }
                break;

            case USART1_HEADER1_STATE:
                if(data==DXL_HEADER2)
                {
                    state = USART1_HEADER2_STATE;
                }
                else
                {
                    state = USART1_INITIAL_STATE;
                }
                break;

            case USART1_HEADER2_STATE:
                packet.id = data;
                state = USART1_ID_STATE;
                break;

            case USART1_ID_STATE:
                packet.paramLength = data - 2;   // -2 because packet length parameter is # params + instruction + checksum
                paramsRead = 0;
                state = USART1_LENGTH_STATE;
                break;

            case USART1_LENGTH_STATE:
                packet.instrErr = data;
                state = USART1_INSTRUCTION_STATE;
                break;

            case USART1_INSTRUCTION_STATE:
                if(packet.paramLength > 0)
                {
                    packet.params[paramsRead] = data;
                    paramsRead++;
                    state = USART1_PARAMETER_STATE;
                }
                else
                {
                    packet.checksum = data;
                    state = USART1_CHECKSUM_STATE;
                }
                break;

            case USART1_PARAMETER_STATE:
                if(paramsRead < packet.paramLength)
                {
                    packet.params[paramsRead] = data;
                    paramsRead++;
                }
                else
                {
                    packet.checksum = data;
                    state = USART1_CHECKSUM_STATE;
                }
                break;

            case USART1_CHECKSUM_STATE:
                // do nothing
                break;

            default:
                state = USART1_INITIAL_STATE;
                break;
            }
            unlockSemaphore(&usart1_rx_lock);

            if(state == USART1_CHECKSUM_STATE)
            {
                // whole packet received
                // process it and act accordingly
                // (most-likely just pass it through to USART0, but we may need to process it internally)

                uint8_t packetOK = validateResponsePacket(&packet);
                if(!packetOK)
                {
                    DXL_Packet errorPacket;
                    errorPacket.instrErr = DXL_CHECKSUM_ERROR;
                    errorPacket.id = packet.id;
                    errorPacket.paramLength = 0;
                    createChecksum(&errorPacket);

                    lockSemaphore(&usart1_tx_lock);
                    transmitPacketUSART1(&errorPacket);
                    unlockSemaphore(&usart1_tx_lock);
                }
                else
                {
                    // if the packet was intended for the controller board then deal with it and send a status packet (if necessary)
                    // otherwise re-transmit the packet over USART 0
                    if(packet.id == CONTROLLER_ID)
                    {
                        // packet was destined for us
                        processPacket(&packet);
                    }
                    else
                    {
                        // re-transmit over USART0 to the Dynamixels
                        lockSemaphore(&usart0_lock);
                        transmitPacketUSART0(&packet);
                        unlockSemaphore(&usart0_lock);

                        if(packet.instrErr == DXL_SYNC_WRITE)
                        {
                            processPacket(&packet);
                        }
                    }
                }

                // fall back to the idle state since the packet's complete
                state = USART1_ID_STATE;
            }
        }
        else
        {
            wait(USART1_RX_EVENT);
        }
    }
}

// interrupt handler for receiving a byte over USART1
// simply stores the byte in the Rx queue
ISR(USART1_RX_vect)
{
    uint8_t data = UDR1;

    rxdLEDOn();
    enqueue(data, &rxQueue_USART1);
    fireEvent(USART1_RX_EVENT);
    rxdLEDOff();
}

// interrupt handler for transmit complete
// dequeues the next byte from the Tx queue and transmits it
ISR(USART1_TX_vect)
{
    // turn on the Tx LED before we transmit
    txdLEDOn();

    if(!txQueue_USART1.empty)
    {
        // dequeue a single byte of data to send
        uint8_t data = dequeue(&txQueue_USART1);
        UDR1 = data;
    }
    else
    {
        txQueue_USART1.txOnEnqueue = TRUE;
    }

    fireEvent(USART1_TX_EVENT);

    // turn off the Tx LED when we're done sending
    txdLEDOff();
}
